/*
    寻迹传感器灯亮是0，表示感应到轨迹，轨迹一般是黑色的胶带。
    利用雷达传感器，如果小车速度过快，由于雷达发射电磁波的暂停原因，
    很容易导致小车偏离轨道，因此改为寻迹传感器或人体红外开关比较合适，
    但寻迹传感器容易受光线的干扰
*/
#define ENA 10  //驱动右轮
#define ENB 11 //驱动左轮
const byte right_motor_pin1 = 2;
const byte right_motor_pin2 = 3;
const byte left_motor_pin3 = 4;
const byte left_motor_pin4 = 5;
const byte right_trace_pin = 8;
const byte left_trace_pin = 12;
const byte trig_pin = 7;
const byte echo_pin = 6;

unsigned int speed = 160;
int direction = 3;  //1:left,2,right,3:forward,4:back()
boolean run1 = true;
unsigned long last_time;
int stop_time = 10;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(right_motor_pin1, OUTPUT);
  pinMode(right_motor_pin2, OUTPUT);
  pinMode(left_motor_pin3, OUTPUT);
  pinMode(left_motor_pin4, OUTPUT);
  pinMode(left_trace_pin, INPUT);
  pinMode(right_trace_pin, INPUT);
  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);
  last_time = millis();
}

void loop() {

  if (run1) {
    go();
  }
  if (millis() > last_time + 500) {
    if (digitalRead(9) == 0) {
      run1 = !run1;
      if (!run1) {
        stop();
        delay(1000);
      }
      last_time = millis();
    }
  }

}

unsigned long ping() {
  digitalWrite(trig_pin, LOW);
  delay(2);
  digitalWrite(trig_pin, HIGH);
  delay(10);
  digitalWrite(trig_pin, LOW);
  return pulseIn(echo_pin, HIGH);
}

void go() {

  delay(5);
  stop();
  delay(2);
  if (digitalRead(left_trace_pin) == HIGH
      && digitalRead(right_trace_pin) == LOW) {
    direction = 2;
  }
  if (digitalRead(left_trace_pin) == LOW
      && digitalRead(right_trace_pin) == HIGH) {
    direction = 1;
  }
  if (digitalRead(left_trace_pin) == LOW
      && digitalRead(right_trace_pin) == LOW) {
    direction = 3;

  }
  switch (direction) {
    case 1:
      turn_left();
      break;
    case 2:
      turn_right();
      break;
    default:
      forward();
  }
}
void forward() {
  analogWrite(ENA, speed);
  analogWrite(ENB, speed);
  digitalWrite(right_motor_pin1, HIGH);
  digitalWrite(right_motor_pin2, LOW);
  digitalWrite(left_motor_pin3, HIGH);
  digitalWrite(left_motor_pin4, LOW);
}
void back() {
  analogWrite(ENA, speed);
  analogWrite(ENB, speed);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, HIGH);
  digitalWrite(left_motor_pin3, LOW);
  digitalWrite(left_motor_pin4, HIGH);
}
void stop() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, LOW);
  digitalWrite(left_motor_pin3, LOW);
  digitalWrite(left_motor_pin4, LOW);
}
void turn_right() {
  analogWrite(ENA, 0);
  analogWrite(ENB, speed);
  digitalWrite(right_motor_pin1, LOW);
  digitalWrite(right_motor_pin2, LOW);
  digitalWrite(left_motor_pin3, HIGH);
  digitalWrite(left_motor_pin4, LOW);
}
void turn_left() {
  analogWrite(ENA, speed);
  analogWrite(ENB, 0);
  digitalWrite(right_motor_pin1, HIGH);
  digitalWrite(right_motor_pin2, LOW);
  digitalWrite(left_motor_pin3, LOW);
  digitalWrite(left_motor_pin4, LOW);
}

//unsigned long ping() {
//  digitalWrite(trig_pin, HIGH);
//  delay(5);
//  digitalWrite(trig_pin, LOW);
//  return pulseIn(echo_pin, HIGH)/58;
//}
